﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.IO;
using OpenTK;
using OpenTK.Graphics.OpenGL;
using Microsoft.Kinect;
//using Microsoft.DirectX;
//using Microsoft.DirectX.Direct3D;

namespace Final_Project
{
    // This class runs major functions of the KinectFusion algorithm.
    class CKinectFusionAlg
    {
        private CIcpAlg m_cIcp;
        private CTsdfAlg m_cTsdf;
       // private Microsoft.DirectX.Vector4[,] ViMatrix;
       // private Microsoft.DirectX.Vector4[,] NiMatrix;
       // private Microsoft.DirectX.Vector4[,] PrevViMatrix;
       // private Microsoft.DirectX.Vector4[,] PrevNiMatrix;
       // private Matrix m_K;
       // private Matrix m_Transform;
       // private Matrix m_PerspectiveMatrix;
       // private float[, ,] m_Model;
        private uint m_unNumOfMapsHandled = 0;
        private int m_nRows;
        private int m_nColumns;
        private OpenGLViewPort viewPort;
      //  private float m_fFront = 400F;
      //  private float m_fBack = 4000F;
        
        public CKinectFusionAlg()
        {
            m_nRows = Convert.ToInt32(CXmlConfigReader.Instance().ReadValueFromElement("DepthMapResolutionRows"));
            m_nColumns = Convert.ToInt32(CXmlConfigReader.Instance().ReadValueFromElement("DepthMapResolutionColumns"));

            GLControl glc = new GLControl();



            /**************************************************/

            /*
            m_cIcp = new CIcpAlg();
            m_cTsdf = new CTsdfAlg();

            ViMatrix = new Microsoft.DirectX.Vector4[m_nColumns, m_nRows];
            PrevViMatrix = new Microsoft.DirectX.Vector4[m_nColumns, m_nRows];
            NiMatrix = new Microsoft.DirectX.Vector4[m_nColumns, m_nRows];
            PrevNiMatrix = new Microsoft.DirectX.Vector4[m_nColumns, m_nRows];

            m_Transform = new Matrix();
            m_K = new Matrix();
            m_Model = new float[512, 512, 512];
            // Set initial Transform matrix values
            m_Transform.M11 = 1;
            m_Transform.M22 = 1;
            m_Transform.M33 = 1;
            m_Transform.M44 = 1;

            // Set K matrix values
            double temp = (2 * Math.Tan(58.5 / 2) * (Math.PI / 180));
            temp = m_nColumns / temp;
            m_K.M11 = (float)(temp);
            m_K.M13 = m_nColumns / 2;
            temp = (2 * Math.Tan(45.6 / 2) * (Math.PI / 180));
            temp = m_nColumns / temp;
            m_K.M22 = (float)(temp);
            m_K.M23 = m_nRows/2;
            m_K.M33 = 1;
            m_K.M44 = 1;

            // build projection matrix
            float fov = 48.6F, aspect = 1.3333F;
            BuildPerspProjMat(fov, aspect, m_fFront, m_fBack);
             * */
        }

        public void AddDepthMapToModel(ref DepthImagePixel[] cCurDepthPixelData, ref short[] cCurDepthShortData)
        {

            /*
            BuildVertexMatrix(ref cCurDepthPixelData);
            if (0 < m_unNumOfMapsHandled)
            {
                BuildNormalsMatrix();
                m_Transform = m_cIcp.RunICP(ref m_Transform, ref ViMatrix, ref PrevViMatrix, ref NiMatrix, ref PrevNiMatrix, ref m_K);
                TransformCurrentViMatrix();
            }
            m_cTsdf.AddMapToModel(ref cCurDepthPixelData, ref ViMatrix, ref m_Model, ref m_K, ref m_Transform);
            //m_cTsdf.Add(ref cCurDepthPixelData, ref ViMatrix, ref m_Model, ref m_K, ref m_Transform);
            if (0 < m_unNumOfMapsHandled)
            {
                StreamWriter writer = new StreamWriter("Rox.model");
                for (int i = 0; i < 512; i++)
                {
                    for (int j = 0; j < 512; j++)
                    {
                        for (int k = 0; k < 512; k++)
                        {
                            if (m_Model[i, j, k] == 1)
                            {
                                writer.WriteLine(i + " " + j + " " + k + " ");
                            }
                        }
                    }
                }
                writer.Close();
            }
            SaveCurrentDepthMapData();
            m_unNumOfMapsHandled++;
             * */
        }

        private void TransformCurrentViMatrix()
        {
            for (uint i = 0; i < m_nColumns; i++)
            {
                for (uint j = 0; j < m_nRows; j++)
                {
                    ViMatrix[i, j].X = m_Transform.M11 * ViMatrix[i, j].X + m_Transform.M12 * ViMatrix[i, j].Y + m_Transform.M13 * ViMatrix[i, j].Z + m_Transform.M14 * ViMatrix[i, j].W;
                    ViMatrix[i, j].Y = m_Transform.M21 * ViMatrix[i, j].X + m_Transform.M22 * ViMatrix[i, j].Y + m_Transform.M23 * ViMatrix[i, j].Z + m_Transform.M24 * ViMatrix[i, j].W;
                    ViMatrix[i, j].Z = m_Transform.M31 * ViMatrix[i, j].X + m_Transform.M32 * ViMatrix[i, j].Y + m_Transform.M33 * ViMatrix[i, j].Z + m_Transform.M34 * ViMatrix[i, j].W;
                }
            }
        }

        private void SaveCurrentDepthMapData()
        {
            for (uint i = 0; i < m_nColumns; i++)
            {
                for (uint j = 0; j < m_nRows; j++)
                {
                    PrevViMatrix[i, j].X = ViMatrix[i, j].X;
                    PrevViMatrix[i, j].Y = ViMatrix[i, j].Y;
                    PrevViMatrix[i, j].Z = ViMatrix[i, j].Z;
                    PrevViMatrix[i, j].W = ViMatrix[i, j].W;
                    PrevNiMatrix[i, j].X = NiMatrix[i, j].X;
                    PrevNiMatrix[i, j].Y = NiMatrix[i, j].Y;
                    PrevNiMatrix[i, j].Z = NiMatrix[i, j].Z;
                    PrevNiMatrix[i, j].W = NiMatrix[i, j].W;

                }
            }
        }

        private void BuildVertexMatrix(ref DepthImagePixel[] cCurDepthPixelData)
        {
            int k = 0;

            m_K.Invert();
            for (int i = 0; i < m_nColumns; i++)
            {
                for (int j = 0; j < m_nRows; j++)
                {
                    if(cCurDepthPixelData[k].Depth > 0)
                    {
                        ViMatrix[i, j].X = 1 * (((i ) * m_K.M11 + (j ) * m_K.M12 + m_K.M13) * cCurDepthPixelData[k].Depth/1000);
                        ViMatrix[i, j].Y = 1 * (((i ) * m_K.M21 + (j ) * m_K.M22 + m_K.M23) * cCurDepthPixelData[k].Depth/1000);
                        ViMatrix[i, j].Z = 1 * (((i ) * m_K.M31 + (j ) * m_K.M32 + m_K.M33) * cCurDepthPixelData[k].Depth/1000);
                        ViMatrix[i, j].W = 1;
                    }
                    k++;
                }
            }
            m_K.Invert();
        }

        
        public void BuildNormalsMatrix()
        {
            Microsoft.DirectX.Vector4 A, B;

            for (int i = 0; i < m_nColumns; i++)
            {
                for(int j = 0; j < m_nRows; j++)
                {
                    int nCordX = i + 1;
                    int nCordY = j + 1;
                    if (nCordX >= m_nColumns || nCordY >= m_nRows)
                    {
                        nCordX--;
                        nCordY--;
                    }
                    A = ViMatrix[nCordX, j];
                    A.Subtract(ViMatrix[i, j]);
                    B = ViMatrix[i, nCordY];
                    B.Subtract(ViMatrix[i, j]);
                    NiMatrix[i, j].X = A.Y * B.Z - A.Z * B.Y;
                    NiMatrix[i, j].Y = -1 * (A.X * B.Z - A.Z * B.X);
                    NiMatrix[i, j].Z = A.X * B.Y - A.Y * B.X;
                }
            }
        } // BuildNormalsMatrix

        public void BuildPerspProjMat(float fov, float aspect, float znear, float zfar)
        {
            double Deg2Rad = Math.PI / 180;
            float xymax = (float)(znear * Math.Tan((fov / 2) * Deg2Rad));
            float ymin = -xymax;
            float xmin = -xymax;

            float width = xymax - xmin;
            float height = xymax - ymin;

            float depth = zfar - znear;
            float q = -(zfar + znear) / depth;
            float qn = -2 * (zfar * znear) / depth;

            float w = 2 * znear / width;
            w = w / aspect;
            float h = 2 * znear / height;

            m_PerspectiveMatrix.M11 = w;
            m_PerspectiveMatrix.M22 = h;
            m_PerspectiveMatrix.M33 = q;
            m_PerspectiveMatrix.M34 = -1;
            m_PerspectiveMatrix.M43 = qn;
        }

    }
}
